function [KScal, domegaIBBiasS, dPSS0] = gyrovelposcalib(velCalPulseSum, posCalPulseRate, rotAngle, KScalExt)
%GYROVELPOSCALIB 按分立级速率标定、位置标定算法对陀螺进行标定
%
% Tips:
% # 不能标定出来的参数将被置为NaN
%
% Input Arguments:
% # velCalPulseSum: m*3矩阵，第i行为第i个速率档位上旋转rotAngle角度陀螺的脉冲输出（单位^），第j列为第j个陀螺的输出
% # posCalPulseRate: n*3矩阵，第i行为第i个位置上的陀螺脉冲输出率（单位^/s），第j列为第j个陀螺的输出
% # rotAngle: m*1向量，各速率档位旋转的角度（绝对值），单位rad，默认全为2π
% # KScalExt: 3*1向量，外部给出的标度因数，当速率档位数不够而不能计算出某个陀螺的标度因数时使用该值，单位^/rad，默认全为NaN
%
% Output Arguments:
% # KScal: 3*1向量，标度因数，单位^/rad
% # domegaIBBiasS: 3*1向量，零偏，单位rad/s
% # dPSS0: 3*3矩阵，安装误差（失准角），单位rad
%
% Assumptions and Limitations:
% # 速率标定时，各次转动需绕惯组X、Y、Z轴中的一个旋转（本函数根据陀螺输出自动判断旋转轴）
% # 速率标定时，需要保证同一个轴正向档位与对应的负向档位转动过程中转角余弦的积分值相同
% # 速率标定时，需要保证转动过程中转角正弦的积分值为0
% # 速率标定时，需要保证同一个轴正向档位与对应的负向档位开始转动时的姿态相同
% # 速率标定时，需要保证同一个轴正向档位与对应的负向档位转动角度相同
% # 速率标定时，需要保证同一个轴正向档位与对应的负向档位转动时间相同
% # 位置标定时，需要保证所有位置上的CNB之和为0
%
% References:
% # 《应用导航算法工程基础》“陀螺三元组误差参数标定算法”

m = size(velCalPulseSum, 1);
if nargin < 4
    KScalExt = NaN(3, 1);
end
if nargin < 3
    rotAngle = 2 * pi * ones(m, 1);
end

% 根据脉冲输出自动判断各次转动所绕的轴（X、Y、Z轴）以及转动方向
pulseSum = zeros(3, 3, 2); % 脉冲输出之和，第1维对应X、Y、Z陀螺，第2维对应旋转轴，第3维对应转动方向
rotAngleSum = zeros(3, 2); % 转动角度之和，行对应旋转轴，列对应转动方向
for i=1:m
    [~, I] = max(abs(velCalPulseSum(i, :)), [], 2);
    if velCalPulseSum(i, I) > 0
        rotAngleSum(I, 1) = rotAngleSum(I, 1) + rotAngle(i, 1);
        pulseSum(:, I, 1) = pulseSum(:, I, 1) + velCalPulseSum(i, :)';
    else
        rotAngleSum(I, 2) = rotAngleSum(I, 2) + rotAngle(i, 1);
        pulseSum(:, I, 2) = pulseSum(:, I, 2) + velCalPulseSum(i, :)';
    end
end

% 计算陀螺的标度因数及失准角
% 构造系数矩阵A
H = [coefmatblk([1 0 0]'); coefmatblk([0 1 0]'); coefmatblk([0 0 1]')];
% 求解方程组
pulseAvg = NaN(3, 3, 2); % 脉冲输出与转动角度之比，第1维对应X、Y、Z陀螺，第2维对应旋转轴，第3维对应转动方向
for i=1:3
    for j=1:2
        if rotAngleSum(i, j) ~= 0
            pulseAvg(:, i, j) = pulseSum(:, i, j) / rotAngleSum(i, j);
        end
    end
end
cBar = reshape((pulseAvg(:, :, 1)-pulseAvg(:, :, 2))/2, 9, 1);
H(isnan(cBar)', :) = 0;
cBar(isnan(cBar)', 1) = 0;
[G, zeroRowIdx] = pinv_nonzero(H);
p = G * cBar;
p(zeroRowIdx, 1) = NaN; % 将无法标定出的参数置无效值
KScal = p(1:3, 1);
KScal(isnan(KScal)) = KScalExt(isnan(KScal));
dPSS0 = vec2offdiag(p(4:9, 1) ./ reshape([KScal KScal]', 6, 1))';

% 计算陀螺的零偏
domegaIBBiasS = mean(posCalPulseRate, 1)' ./ KScal;
end

function [HBlk] = coefmatblk(uB)
HBlk = [uB(1, 1) 0 0 uB(2, 1) uB(3, 1) 0 0 0 0
    0 uB(2, 1) 0 0 0 uB(1, 1) uB(3, 1) 0 0
    0 0 uB(3, 1) 0 0 0 0 uB(1, 1) uB(2, 1)];
end